#include <iostream>
#include <ros/ros.h>
#include <string.h>
#include <geometry_msgs/Vector3Stamped.h>
#include <geometry_msgs/PoseStamped.h>
#include "std_msgs/Float64.h"
#include <eigen3/Eigen/Dense>
#include <signal.h>

using namespace std;

/* -------------------------------------------------------------------------- */
/*                                 Global Var                                 */
/* -------------------------------------------------------------------------- */

constexpr double ANGLE2SERVO = 4096.0 / 360.0;
constexpr double SERVO2ANGLE = 360.0 / 4096.0;
constexpr double ANGLE2RAD = M_PI / 180.0;
constexpr double SERVO2RAD = SERVO2ANGLE * ANGLE2RAD;
constexpr double RAD2SERVO = 1.0 / SERVO2RAD;

ros::Publisher servo_state_pub;
ros::Publisher camera_pos_pub;

Eigen::Matrix3d base2cam;
Eigen::Vector3d t_body2base_body{0.0, 0.0, 0.0};

double servo_pos = 0.0;

/* -------------------------------------------------------------------------- */
/*                                  function                                  */
/* -------------------------------------------------------------------------- */

void set_angle_cb(const geometry_msgs::PoseStamped::ConstPtr &msg)
{
  // add minus sign to make the servo rotate in the right direction
  servo_pos = msg->pose.position.x * ANGLE2RAD;
}

void set_rad_cb(const geometry_msgs::PoseStamped::ConstPtr &msg)
{
  // add minus sign to make the servo rotate in the right direction
  servo_pos = msg->pose.position.x;
}

void pub_pos_cb(const geometry_msgs::PoseStampedConstPtr &msg)
{
  geometry_msgs::PoseStamped cam_pose;
  cam_pose.header.stamp = ros::Time::now();

  Eigen::Matrix3d body2base =
      Eigen::AngleAxisd(servo_pos, Eigen::Vector3d(0.0, 0.0, 1.0)).toRotationMatrix();
  Eigen::Quaterniond q_body2cam(body2base * base2cam);
  Eigen::Quaterniond q_world2body =
      Eigen::Quaterniond(msg->pose.orientation.w,
                         msg->pose.orientation.x,
                         msg->pose.orientation.y,
                         msg->pose.orientation.z);
  Eigen::Quaterniond q_world2cam = q_world2body * q_body2cam;

  Eigen::Vector3d t_body2base_world = q_world2body * t_body2base_body;

  cam_pose.pose.position.x = msg->pose.position.x + t_body2base_world(0);
  cam_pose.pose.position.y = msg->pose.position.y + t_body2base_world(1);
  cam_pose.pose.position.z = msg->pose.position.z + t_body2base_world(2);
  cam_pose.pose.orientation.w = q_world2cam.w();
  cam_pose.pose.orientation.x = q_world2cam.x();
  cam_pose.pose.orientation.y = q_world2cam.y();
  cam_pose.pose.orientation.z = q_world2cam.z();
  camera_pos_pub.publish(cam_pose);
}

void servo_pub_timer_callback(const ros::TimerEvent &e)
{
  geometry_msgs::Vector3Stamped servo_pos_msg;
  servo_pos_msg.header.stamp = ros::Time::now();
  servo_pos_msg.vector.x = 180 - servo_pos * RAD2SERVO;
  servo_state_pub.publish(servo_pos_msg);
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "servo_read_write");
  ros::NodeHandle nh;

  bool whether_pub_servo_state;
  int pub_rate;

  const std::string &node_name = ros::this_node::getName();
  nh.param("servo/pub_state", whether_pub_servo_state, true);
  nh.param("servo/pub_rate", pub_rate, 50);
  nh.param(node_name + "/" + "servo/x_off", t_body2base_body(0), 0.0);
  nh.param(node_name + "/" + "servo/y_off", t_body2base_body(1), 0.0);
  nh.param(node_name + "/" + "servo/z_off", t_body2base_body(2), 0.0);

  ROS_INFO_STREAM("[SERVO] offset: " << t_body2base_body.transpose());

  // ros::Subscriber set_angle_sub =
  //     nh.subscribe<geometry_msgs::PoseStamped>("/servo/set_angle", 1, &set_angle_cb);
  // ros::Subscriber set_rad_sub =
  //     nh.subscribe<geometry_msgs::PoseStamped>("/servo/set_rad", 1, &set_rad_cb);

  ros::Subscriber cam_base_pose_sub =
      nh.subscribe<geometry_msgs::PoseStamped>("/cam_base_pose", 1, &pub_pos_cb);

  servo_state_pub = nh.advertise<geometry_msgs::Vector3Stamped>("/servo/state", 1);
  camera_pos_pub = nh.advertise<geometry_msgs::PoseStamped>("/camera_pose", 1);

  ros::Timer servo_pub_timer;
  if (whether_pub_servo_state)
  {
    servo_pub_timer =
        nh.createTimer(ros::Duration(1 / pub_rate), servo_pub_timer_callback);
  }

  // init
  base2cam << 0.0, 0.0, 1.0,
      -1.0, 0.0, 0.0,
      0.0, -1.0, 0.0;

  ros::spin();

  return 0;
}
